#include "Kinematic.h"
#include "Steering.h"
#include "Vectores.h"

#include <cmath>
#include <iostream>
#include <vector>
#include "CApp.h"

Kinematic::Kinematic(){
  position[0] = 0;
  position[1] = 0;
  position[2] = 0;
  velocity[0] = 0;
  velocity[1] = 0;
  velocity[2] = 0;
  rotation    = 0;
  orientation = 0;
  // Inicializadas a on valor "alto"
  max_velocity = 1000000;
  max_rotation = 1000000;
}

Kinematic::Kinematic(float pos[3]){
  position[0] = pos[0];
  position[1] = pos[1];
  position[2] = pos[2];
  velocity[0] = 0;
  velocity[1] = 0;
  velocity[2] = 0;
  rotation    = 0;
  orientation = 0;
  // Inicializadas a on valor "alto"
  max_velocity = 1000000;
  max_rotation = 1000000;
 
}

void  Kinematic::update(Steering* st,long int time){
  float vel, pi2;
  Platform* p = theApp.getPlatform(position);
  this->position[0] += this->velocity[0]*time;
  this->position[1] += this->velocity[1]*time;
  this->position[2] += this->velocity[2]*time;
  this->orientation += this->rotation*time;

  this->velocity[0] += st->linear[0]*time;
  this->velocity[1] += st->linear[1]*time;
  this->velocity[2] += st->linear[2]*time;
  this->rotation += st->angular*time;

  // Ajuste de gravedad
  if (p == NULL){
    if(this->position[2] < 0){
      this->position[2] = 0;
      this->velocity[2] = 0;
    }
  }else{
    if(this->position[2] < p->position[2]){
      this->position[2] = p->position[2];
      this->velocity[2] = 0;
    }
  }

  vel = length(this->velocity);
  if(vel > max_velocity){
    normalize(this->velocity);
    multVectorScalar(this->velocity, max_velocity);
  }

  if(abs(this->rotation) > max_rotation){
    this->rotation = ((this->rotation<0)?-1:1) * max_rotation;
  }

  // Ajustes de la orientacion al rango [-2PI   2PI]
  pi2 = 2.0f*M_PI;

  // DEPRECATED: las siguientes 2 lineas
//    while(pi2<this->orientation) this->orientation -= pi2;
//    while(this->orientation<-pi2) this->orientation += pi2;

  // JORMAR: vamos a intentar darle un rango [-PI  PI]
  while(M_PI<this->orientation) this->orientation -= pi2;
  while(this->orientation<-M_PI) this->orientation += pi2;
}

vector<int> Kinematic::getOrientationVector(){
  vector<int> ret(3,0);
  ret[0] = sin(this->orientation);
  ret[1] = cos(this->orientation);
  return ret;
}

float Kinematic::getNewOrientation(float co,float v[3]){
    return 0.0f;
}


void Kinematic::printKinematic(){
  cout << "x " << position[0] << " y " << position[1] << endl;
}
